#include "IPPNS_1034/common_USB_serial.h"


namespace IPPNS{
   
    
    void COMMON_USB_SERIAL::serialInit()
    {
        if(Ros_to_Stm32.isOpen())
        {
        ROS_INFO("serial port has already opened");
            ser_open_flag=true;
        }
        else 
        {
            Ros_to_Stm32.setPort(this->ser_port);//选择要开启的串口号
            Ros_to_Stm32.setBaudrate(this->baudrate);//设置波特率
            serial::Timeout _time =serial::Timeout::simpleTimeout(this->time_out);//超时等待
            Ros_to_Stm32.setTimeout(_time);

            if(Ros_to_Stm32.isOpen())
            {
                ROS_INFO("serial port has openictrl_flagng");
                ser_open_flag=true;
            }else{
                try
                {
                    Ros_to_Stm32.open();//如果没打开，才打开串口
                    ROS_INFO("serial port is opening");
                    ser_open_flag=true;
                }
                catch(const std::exception& e)
                {
                    ROS_INFO("serial port has down");
                    ser_open_flag=false;
                    std::cerr << e.what() << '\n';
                }
               
            }
        } 
    }
 /*
    @brief:通过串口向下位机发送数据
    @input：发送数组指针，数组长度
    @output:void；
    */
    void COMMON_USB_SERIAL::writeData(const uint8_t *buf,int Size)
    {
        try
        {
            Ros_to_Stm32.write(buf,Size);
            ROS_INFO("Succsessfully send");
        }
        catch(const std::exception& e)
        {
            ROS_INFO("Unsuccsessfully send");
            std::cerr << e.what() << '\n';
        }
    }

    /*
    @brief:通过串口读取下位机数据，并存储到数组里
    @input:void
    @output:返回数组指针与数组长度；
    */
    std::pair<uint8_t*, int> COMMON_USB_SERIAL::receiveData()
    {
        //接收前确定接收的长度
        uint8_t buf_re[150]={};
        int length=0;
        size_t Receive_N =Ros_to_Stm32.available();//获取缓冲区内的字节数
        if (Receive_N !=0)
        {
            length=Ros_to_Stm32.read(buf_re,sizeof(buf_re));
        }
        return std::make_pair(buf_re, length);
    }


    //四元数 -->> 欧拉角
    geometry_msgs::Vector3 Command_Trans::tf_quaternion2RPY(const geometry_msgs::Quaternion& orientation)
    {
        tf::Quaternion quat;
        geometry_msgs::Vector3 Euler;
        tf::quaternionMsgToTF(orientation ,quat);
        tf::Matrix3x3(quat).getRPY(Euler.x,Euler.y,Euler.z);
        return Euler;
    }

    
    tf::Quaternion Command_Trans::tf_quaternion2XYZW(const geometry_msgs::Vector3& Euler)
    {
        tf::Quaternion qtn;
        qtn.setRPY(Euler.x,Euler.y,Euler.z);// 设置偏航值，俯仰值，翻滚值，单位是弧度
        return qtn;
    }

    
    //实例化一个全局联合体
    Exchange ex_u8;
    //转换模板
    /*
    function:利用联合体的特性，将浮点数转换为uint8_t的字符串形式
    intput:arg1输入浮点数，arg2输入数组指针
    ouput:将转换的uint8_t数组复制到外部数组，并输出转换数组的长度lenarray。
    */
    template <typename T>
    int conv(const T value,uint8_t *buffer_ptr) {
        ex_u8.target_float = value;
        memcpy(buffer_ptr, &ex_u8.target_u8, sizeof(ex_u8.target_u8));
        return sizeof(ex_u8.target_u8)/sizeof(ex_u8.target_u8[0]);
    }


    std::pair<uint8_t*, int> Command_Trans::Twist2data(const geometry_msgs::Twist& vel_msg)
    {
        uint8_t buffer[28] = {};
        uint8_t* ptr = buffer;
        int pos=0;//作为数组复制的位置
        //赋值标头
        memcpy(ptr,&this->handle.twist_,sizeof(this->handle.twist_));
        ptr+=sizeof(this->handle.twist_);//p2
        //X方向直线速度
        ptr+=conv<float>(vel_msg.linear.x,ptr);//p6
        //y方向直线速度
        ptr+=conv<float>(vel_msg.linear.y,ptr);//p10
        //z方向直线速度
        ptr+=conv<float>(vel_msg.linear.z,ptr);//p14
        //x方向的旋转速度
        ptr+=conv<float>(vel_msg.angular.x,ptr);//p18
        //y方向的旋转速度
        ptr+=conv<float>(vel_msg.angular.y,ptr);//p22
        //z方向的旋转速度
        ptr+=conv<float>(vel_msg.angular.z,ptr);//p26
        // //赋值标尾
        memcpy(ptr,&this->handle.ender,sizeof(this->handle.ender));//p28
        //发送速度数据
        return std::make_pair(ptr, sizeof(buffer));
    }


    std::pair<uint8_t*, int> Command_Trans::Pose2data(const geometry_msgs::Pose& pose_msg)
    {
        uint8_t buffer[28] = {};
        uint8_t* ptr = buffer;
        //赋值标头
        memcpy(ptr,&this->handle.pose_,sizeof(this->handle.pose_));
        ptr+=sizeof(this->handle.pose_);//p2
        //X方向位置
        ptr+=conv<float>(pose_msg.position.x,ptr);//p6
        //y方向位置
        ptr+= conv<float>(pose_msg.position.y,ptr);//p10
        //z方向位置
        ptr+= conv<float>(pose_msg.position.z,ptr);//p14
        //X方向角度
        ptr+=conv<float>(pose_msg.orientation.x,ptr);//p18
        //y方向角度
        ptr+= conv<float>(pose_msg.orientation.y,ptr);//22
        //z方向角度
        ptr+= conv<float>(pose_msg.orientation.z,ptr);//26
        // //赋值标尾
        memcpy(ptr,&this->handle.ender,sizeof(this->handle.ender));//p28
        //发送位置数据
        return std::make_pair(ptr, sizeof(buffer));
    }

}

